/**
 * @file	Test_Mouse_Radio.c
 * @author	Mitch, David
 * @brief
 */


#include "../header/common.h"
#include "../roomba/roomba.h"
#include "../roomba/roomba_sci.h"
#include "../uart/uart.h"
#include "../radio/radio.h"
#include <util/delay.h>
#ifdef TEST_RADIO_MOUSE


#define     clock8MHz()    CLKPR = _BV(CLKPCE); CLKPR = 0x00;
const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;
void roomba_Drive( int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

uint8_t station1_addr[5] = { 0xED, 0xB7, 0xED, 0xB7, 0xED };
volatile uint8_t rxflag = 0;
radiopacket_t packet;

int main(void) {

	clock8MHz();
	cli();
	//initialize leds
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);

	Roomba_Init();
	Radio_Init();
	Radio_Configure_Rx(RADIO_PIPE_0, station1_addr, ENABLE);
	Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);

	sei();
	roomba_Drive(100, 0x0001);

	for (;;)
		{
			if (rxflag)
			{
				_delay_ms(20);
				roomba_Drive(0, 0);
				//PORTD ^= _BV(PD4);
				//PORTD ^= _BV(PD5);
				// Copy the received packet into the radio packet structure.  If there are no more packets,
				// then clear the rxflag so that the interrupt will set it next time a packet is received.
				if (Radio_Receive(&packet) != RADIO_RX_MORE_PACKETS)
				{
					rxflag = 0;
				}

				// If the packet is not a command, blink an error and don't do anything.
				if (packet.type != COMMAND)
				{
					//PORTD ^= _BV(PD7);
					continue;
				}

				if (packet.payload.command.command == START ||
						packet.payload.command.command == BAUD ||
						packet.payload.command.command == CONTROL ||
						packet.payload.command.command == SAFE ||
						packet.payload.command.command == FULL ||
						packet.payload.command.command == SENSORS)
				{
					// Don't pass the listed commands to the Roomba.
					continue;
				}

				// Set the radio's destination address to be the remote station's address
				Radio_Set_Tx_Addr(packet.payload.command.sender_address);

				// Update the Roomba sensors into the packet structure that will be transmitted.
				Roomba_UpdateSensorPacket(1, &packet.payload.sensors.sensors);
				Roomba_UpdateSensorPacket(2, &packet.payload.sensors.sensors);
				Roomba_UpdateSensorPacket(3, &packet.payload.sensors.sensors);

				// send the sensor packet back to the remote station.
				packet.type = SENSOR_DATA;

				if (Radio_Transmit(&packet, RADIO_WAIT_FOR_TX) == RADIO_TX_MAX_RT)
				{
					//PORTD ^= _BV(PD4);	// flash red if the packet was dropped
				}
				else
				{
					//PORTD ^= _BV(PD5);	// flash green if the packet was received correctly
				}
			}
		}

		return 0;
	}

void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
	//PORTD ^= _BV(PD7);
}


#endif
